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ABSTRACT. This paper presents algorithms for computing constraints on the position of an object 
due to the presence of obstacles. This problem arises in applications which require choosing how 
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of the object. The configurations forbidden to this object, due to the presence of obstacles, can then 
be characterized as regions in the Configuration Space. Hie paper presents algorithms for computing 
these Configuration Space obstacles when the objects and obstacles are polygons or polyhedra. An 
approximation technique for high-dimensional Configuration Space obstacles, based on projections of 
obstacles slices, is described. 
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Introduction 



1. Inlroduction 

An increasing range of computer applications deal with models of two- and three-dimensional 
objects. In these applications, objects must often be placed among other objects or moved in such 
a way so that no interference from, nearby objects result. In this paper, these types of problems 
are called spatial planning problems. Among the many applications where spatial planning plays an 
important role are: 

1. Planning the layout of a building [8], i.e. the arrangement of walls, corridors, rooms, and 
equipment so as to fulfill a user's design constraints as well as implicit consistency constraints. 

2. Planning how to machine a part using a Numerically Controlled Machine Tool [47], which 
requires plotting the path of one or more cutting surfaces so as to produce the desired part. 

3. Planning the layout of an IC chip [45] so as to minimize area, subject to geometric design 
constraints. 

4. Planning how to assemble a part using an industrial robot [18] [19] [38], which requires 
choosing how to grasp objects, move them without collisions and bring them into contact. 

Problems in spatial planning generally involve (at least) two important types of considerations: 

1. Geometric — The legal solutions must be characterized, which involves considering inter- 
actions between the shapes of objects and obstacles. 

2. Optimization — The best solution must be chosen from among the legal solutions. 

This paper deals primarily with computing constraints on the position of an object due to the presence 
of obstacles, thus its focus is on the geometric aspect of spatial planning. The development throughout 
will be based on polyhedral object models, although many of the results and approaches are ap- 
plicable to other classes of object models, 

Recently, there has been a rapid growth of interest in efficient algorithms for geometric problems. 
Previous work J has focused on algorithms for (1) computing convex hulls [9] [12] [15] [29], (2) inter- 
secting convex polygons and polyhedra [5] [24] [33] [35], (3) intersecting half-spaces [7] [30] (4) decom- 
posing polygons [32], and (5) closest-point problems [34]. This paper formulates two other geometric 



] The references cited here are representative of the current literature; they are by no means a complete survey. 
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Spatial Planning Problems 



problems, Findspacc and I ; indpath, with important applications in spatial planning, describes an 
approach to their solution and presents algorithms for the central problem posed by the approach. 
Previous work on these problems is briefly reviewed in Section 11. 



2. Spatial Planning Problems 

Let R be a convex polyhedron that contains k n other, possibly overlapping, convex polyhedra Bj 
designated as obstacles. Let/1 be the union of k A convex polyhedral*, i.e. A = \J k i l ll A l . Figure 
1 illustrates two related geometric problems, defined below (where position is used to mean both 
translation and orientation): 

1. Findspacc — Find a position for 4, inside R, such that ViV; :AiC\Bj = 0. This is called a 
supposition. 

2. Findpath — Find a path for 4 from position 5 to position g such that 4 is always in R and A 
never overlaps any of the B{. This is called a safe path. 

Versions of the Findpath and Findspacc problems occur in many spatial planning applications. 
For example, the approaches to object layout (template packing) in [1] [2] [3] and [10] are based on 
solutions to Findspacc in two-dimensions. Also, systems for programming industrial robots using 
object models [20] [38] must solve 3-dimcnsional Findspacc and Findpath problems when choosing 
grasp points on objects or paths for die robot. 



3. The C space Approach to Spatial Planning 

In this section, an overview of the Configuration Space approach to spatial planning will be 
presented. Sections 5 through 10 discuss the approach more formally. 

The position and orientation of a rigid solid can be represented as a single 6-dimcnsional point, 
called its configuration. The 6-dimcnsional space of configurations for a solid, A, is called its 
Configuration Space and denoted Cspace A . For example, a configuration may have one coordinate 
value for each of the x, y, z coordinates of a selected point on the object and one coordinate value for 
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Figure 1. The definition offl.R- and 4 for Findspace and Findpath problems in two dimen- 
sions, (a) The Findspace problem is to find a position for A which does not overlap any of the 
Bj. (b) The Findspace problem is to find a path for A from 5 to g that avoids theB?. 
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Figure 2. The Cspace A obstacle due to B, for fixed orientation of A. 

each of the object's Eulcr angles 2 . In general/an n-dimcnsional configuration space can be used to 
model any system that can be characterized with n parameters. An example is die configuration of 
an industrial robot with n joints, where n is typically 5 or 6. In Cspace A , die set of configurations 
where A overlaps B will be denoted CO A (B), the Cspace A Obstacle due to B. Similarly, those 
configurations where A is completely inside B will be denoted CI A (B), the Cspace A Interior of B. 
Together, these two Cspace A constructs embody all the information needed to solve Findspace and 
Findpath problems. 

If the orientation of a convex polygon A is fixed, Cspace A is simply the [x } y) plane. This 
is so because the (x, y) position of some reference vertex rv A is sufficcnt to specify the polygon's 
configuration. In this case, the presence of another convex polygon B constrains rv A to be outside 
of CO A (B), a larger convex polygon, shown as the shaded region in Figure 2. Thus, the Findspace 
problem can be transformed to the equivalent problem of placing rv A outside of CO A (B), but inside 
CI A (R), Similarly, for multiple obstacles B u a location for A is safe iff rv A is not inside any of the 



The relative rotation of one coordinate system relative to another can be specified in terms of three angles usually 
referred to as Euler angles [43]. These angles indicate the magnitude of three successive rotations about specified axes, 
but no uniform convention for the choice of axes exists. 
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Figure 3. The Findpath problem and its formulation using the CO^ y (Bj). The shortest collision- free paths 
connect the origin and the destination via the vertices of the CO polygons. 



CO A (Bi), but is inside CI A (R). Subsequent sections discuss algorithms for CO A (B) and CI A (B). 

If the orientation of A is fixed, then the Findpath problem for the polygon A among the B, is 
equivalent to the Findpath problem for flic point rv A among the CO A (B t ). When the CO A (Bi) are 
polygons, the shortest 3 safe paths for rv A arc piecewise linear paths connecting the start and the 
goal via the vertices of the CO polygons, Figure 3. Therefore, Findpath can be formulated as a 
graph search problem. The graph is formed by connecting all pairs of CO vertices (and the start 
and goal) that can "see" each other, i.e. can be connected by a straight line that does not intersect 
any of die obstacles. The shortest path from the start to the goal in this visibility graph (Vgraph) is 
die shortest safe path for A among die B { [21]. This algorithm efficiently solves Findpath problems 
when the orientation of A is fixed, but the paths it finds are very susceptible to inaccuracies in the 
object model. These padis touch the Cspace A obstacles, dierefore if the model were exact, an object 
moving along this type of path would just touch the obstacles. But an inaccurate model may result in 
a collision. Furthermore, the Vgraph algorithm with three-dimensional objects and obstacles does not 
find optimal paths. 
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3 This assumes Euclidean distance as a metric. For the optimally conditions using a rectilinear (Manhattan) metric, see 
[16]. 
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Figure 4. Slice projections of C space A obstacles computed using the (j, y)-area swept out by A over a 
range of values. Ilach of the shaded obstacles is the (.r, ?y)-projection of a fl-s!ice of CO A {B). ITie figure 
also shows a polygonal approximation to die slice projection and the polygonal approximation to the swept 
volume from which it derives. 



When A is a three-dimensional solid which is allowed to rotate, CO X {B) is a complicated curved 
object in a 6-dimcnsional Cspace A . Rather than compute these objects directly, the approach taken 
here is to use a sequence of two- and three-dimensional projections of the high-dimensional Cspace A 
obstacles. In particular, the 6-dimcnsional Cspace A obstacles for a rigid solid can be approximated 
by several 3-dimcnsional projections of CO slices. A j-slicc of an object C £ 3ft n is defined to be 
{ (A, . . . , p n ) G C | 7j < Pj < ij }, where 7./ and ^ are tlic lower and upper bounds of the slice, 
respectively. Then, if # is a set of indices between 1 and n, a /f -slice is the intersection of all the j- 
slices for j G K. Notice that a /f-slice of C is an object of the same dimension as C. Slices can then 
be projected onto those coordinates not in K to obtain objects of lower dimension. 

As an example of the slice projection technique, Figure 4 shows, shaded, the (x, y) projection of 
^-slices of CO A {B) when A and B are convex polygons. These slices represent configurations where 
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A overlaps B for some orientation of A in the specified range of 6. Section 10 shows that these slice 
projections are the Cspace A obstacles of the area (volume) swept out by A over the range of orienta- 
tions of the slice. Note that approximating the swept volume as a polyhedron leads to a polyhedral 
approximation for the projected slices, as shown in Figure 4. 
The slice projection technique has two important properties: 

1. A solution to a Findspace problem in any of the slices is a solution to the original problem, 
but since the slices are an approximation to the C space obstacle, the converse is not necessarily 
true. 

2. The slice projection of Cspace A obstacle can be computed without having to compute the 
high-dimensional Cspace A obstacle, see Section 10. 

The slice projection method can also be used to extend the Vgraph algorithm described earlier to 
find safe (but sub-optimal) paths when rotations of A are allowed [21]. A number of slice projections 
of the Cspace obstacles arc constructed for different ranges of orientations of A. The problem of 
planning safe paths in the high-dimensional Cspace A is decomposed into (a) planning safe paths 
via CO vertices within each slice projection and (b) moving between slices, at configurations that 
arc safe in both slices. Both of these types of motions can be modelled as links in the Vgraph, 
therefore the complete algorithm can be formulated as a graph search problem. This approach is 
illustrated in Figure 5. However, the Vgraph algoridim has several drawbacks when the obstacles are 
3-dimensional. In particular, 

1. Optimal padis in higher dimensions do not typically traverse the vertices of the Cspace 
obstacles. 

2. In higher dimensions, there may be no paths via vertices, within the enclosing polyhedral 
region R, although other types of safe paths within R may exist. 

These drawbacks may be alleviated by introducing additional nodes in the Vgraph which do not 
correspond to vertices [21]. An alternative strategy to finding safe paths in Cspace A is discussed in 
[20]. 

The key to the Cspace approach outlined above is computing the Cspace obstacles; the rest of 
the paper is devoted to this problem. 
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Figure 5. An illustralion of the Findpath algorithm using slice projection described by I .ozano-Pcrcz and 
Wesley in [21]. A number of slice projections of the ("space obstacles are constructed for different ranges 
of orientations of A. Jlie problem of planning safe paths in (he high-dimensional C space A is decomposed 
into (3) planning safe paths via CO vertices within each slice projection and (2) mo\ing between slices, at 
configurations that arc safe in both slices. A\ represents A in its initial configuration. A 3 represents A in its 
final configuration and A 2 is a simple polyhedral approximation to the swept volume of A between its initial 
and final orientation. 



4. Notation and Conventions 
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All geometric entities — points, lines, edges, planes, faces and objects — will be treated as 
(infinite) sets of points. All of these entities will be in 3J fc , the /c-dimcnsional real Euclidean space, a, 
6, x, and y shall denote points of Sl k \ as well as the corresponding vectors. A, J3, and C shall denote 
sets of points in 5R fc , while / and K shall denote sets of integers. 7, 9, and /3, shall denote reals, while 
n, ij } k shall be used for integers. The coordinate representation of a point c G & n , for any n, shall 
be c = (7,) = (7 b . . . , 7 n ). The magnitude of a vector a will be ||a|| and the cardinality of a set A 
will be |i4|. The scalar (dot) product of vectors a and b will be denoted (a, 6). 

The following operations are defined on sets of points in 9J n : . 
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Figure 6. I inked Polyhcdra can be used to model the gross geometry of manipulators. 
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A®B = {a + b\aEA,beB} 
QA = {-a\aEA} 
If a set A consists of a single point a, then a0B = { a } © B = A ©B. Also, ,4 ©5 = yi © (©#). 

Note that, typically A® A ^ {2a\ a E A) and ^©,4^0, alUiough ,4 © B = B © A. The set 
difference and set complement operations will be denoted A—B and —A respectively. 

Rigid objects will be represented as sets of possibly overlapping convex polyhcdra since this repre- 
sentation simplifies the algorithms for computing CO. Theorem 1 below follows directly from the 
definition of CO; it justifies the use of this object representation. 



\k A 
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Theorem 1: If A = \J*LiAi andB = {JjLiBf 

CO A (B) = |J (J CO Ai {Bj). 

The position and orientation of a polyhedron will be defined relative to an initial position and 
orientation. In this initial position, some vertex of the polyhedron coincides with the origin of the 
global coordinate frame. For a polyhedron P, this vertex is called the reference vertex ofP, or rvp. 

In the sequel, a different kind of object, called Linked polyhcdra, is used. These objects are 
kinematic chains with polyhedral links and prismatic or rotary joints 4 Figure 6. The relative position 

4 Joinis arc represented abstractly, i.e. their representation as polyhedra do not determine their motion properties. In 
particular, some values of the joint parameters may cause overlap of adjacent links. 
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and orientation of adjacent links, A, and A i+[ , is determined by the i ih joint parameter (angle) [26]. 
The set of joint parameters of a linked polyhedron completely specifies the position and orientation of 
all the links. 

5. Configuration Space and C space Obstacles 

The configuration of a fc-dimcnsional polyhedron, A, is a point a = (71, . . . , 7/) £ 5R d , with d — 
k -f (2); where (7,, . . . , 7 fc )is the position ofrv A and (7^+1, . . . , 7/) are the Fuller angles specifying 
the orientation of A relative to its initial orientation. The configuration of a linked polyhedron having 
d joints is the d-vector of the joint parameters. The rf-dimensional space of configurations of A is 
denoted Cspace A . A in configuration x is (A) x \ A in its initial configuration is (A) . 

The fundamental observation about Configuration Space is that, in Cspace A , die (A) x is repre- 
sented by the vector x. Given this, the basic problem in the Cspace approach to spatial planning is to 
define how the obstacles B { map into Cspace A . The mapping chosen here exploits two fundamental 
properties of objects: Their rigidity, which allows their configurations to be characterized by a few 
parameters, and their solidity, which requires that a point not be inside more than one object. 

Definition: The Cspace A obstacle due to 5, denoted CO A [B\ is defined as follows: 

CO A (B) = {xE C space A \ {A) x C]B^0} 

Thus, if a; £ CO A (B) then (A) x and B cither touch or overlap. Conversely, any configuration 
x £ CO A (X) (for all obstacles X) is safe. The following defines a Cspace A entity complementary to 
CO A {B). 

Definition: The C space A interior of B, denoted CI A (B), is defined as follows: 

CI A (B) = {xE Cspace A \ [A) x QB} 

The sets CO A (B) and CI A (B) are difficult to compute and manipulate since they are curved, 
6-dimcnsional objects when A and B are polyhcdra. Instead, this paper will deal with projections 
of slices and cross-sections 5 of CO A {B). For example, for fixed Euler angles of A, the Cspace A 

5 A cross-section is a slice whose lower and upper bounds are equal. 
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obstacle due to B is denoted CO\ yz (B) indicating that it is a set of xyz positions, rather than the 
full configurations. CO^ !Z {B) is the projection onto the x } y } z eordinates of an Ruler angle cross- 
section 6 In general, the superscript to CO and CI will indicate the composition of their members, 
e.g. CO^{B) and CO^°{B) denote sets of (a:, y) and (x, y, 0) values respectively. 



6. The Sliding Algorithm for CO xy 

This section presents a "naive" algorithm for computing CO xy {B) when A and B are convex 
polygons and the orientation of A is fixed. In subsequent sections, more efficient algorithms are 
presented for this case. 

When die orientation of A is fixed, the configurations of A are simply the positions of the 

reference vertex rv A . Clearly, the boundary of CO A (B) is the locus ofrv A where A just touches R 

This suggests a simple algorithm for computing CO^ J (B): slide A around die perimeter of B and 

f*\ trace the path of rv A . The term sliding is merely suggestive; in practice, knowing which vertex first 

touches an edge completely defines the path of rv A over that edge. 

The central step of this Sliding algorithm is to determine what point (or edge) of A first contacts 

each edge of B and vice versa, Figure 7. The normal vector of each edge of B defines an approach 

direction for the edge, ff A is moving from a great distance towards an edge of 23 along the normal 

direction, and no edge of A is parallel to one of 5, then contact will first happen at a vertex of A. This 

contact vertex is the one with the minimum perpendicular distance to die edge of B. As this vertex 

"slides" along die edge of 23, rv A traces an edge of CO% J {B) which is parallel to the edge of B and 

of equal length. But this edge is displaced by the distance from the contact vertex to rv A , projected 

along the normal to the edge of R Interchanging die roles of A andS shows that each edge of A and 

some vertex of B gives rise to an edge of CO% J (B). This new edge is traced out by rv A as the edge 

of A "slides" along the contact vertex of 23; dierefore, die new edge is parallel to die edge of A and 

of equal length, but displaced from the contact vertex of B by the perpendicular distance between the 

edge and rv A . If A and B have pairs of parallel edges, \hcn CO^(B) will have a parallel edge for 

each such pair, displaced as above, but whose length is die sum of the two edges. 

X"*\ 6 When A*s orientation is fixed, we assume without loss of generality that A is in its initial orientation, i.e. A is simply 

(/4)o displaced by some 3-vector x. 
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Figure 7. Illustration of Sliding algorithm, cv are contact vertices. 

The Sliding algorithm needs 0(\edges(A)\ X \edges{B)\) operations to compute CO%*(B) for 
convex A and J3. This is not optimal; Section 8 describes an 0(\edges(A)\ -f- \edges(B)\) algorithm 
for this task. 



The Sliding algorithm derives the edges of CO% J (B) from the interaction of one of (a) an edge 
of B and a vertex of A, (b) an edge of A and a vertex of S, or (c) an edge of A and an edge of 
B. Similarly, each face of CO^ yz (B), for A and B convex polyhedra, can be computed from the 
interaction of one of 

a. a face of A and a vertex of B or a face of B and a vertex oM, 

b. a face of A and a coplanar edge of B or vice versa, 
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Figure 8. Illustration of the Proof of Theorem 2. Any location of rv A . in this case t\ for which A and B have a 
point in common (expressible as b and a'), can be expressed as c = b — a' . Therefore, CO^ yz (B) = BQ(A} . 



c. an edge of A and a non-parallel edge of 5, 

d. a face of /t and a coplanar face of B. 

7. Vector Set Sums and Configuration Space Obstacles 

The fundamental result of this section is the following: 

Theorem 2: For ,4 and B, sets in R 3 , COf 2 (B) =BQ (A) . 

Proof: If c is an (x, y y z) configuration of A then (A) c = c® (4) . Therefore, if a £ (4) c then 
a = a'-fc, where a' G (/i) , sec Figure 8. If b £ Bn(/l) C) then 6 = a'-fc and therefore c = 6— a'. 
Clearly, the converse is also true. 



^m^ 



If A and B are convex then A £ and A © B are also convex [13 p.9], therefore CO^ z {B) is 
convex. Also, for J3 and A in their initial configurations, 

C02*t(A)o) = (04)o (B)o) = eCO* A y*{(B) ). 
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Figure 9. Characterization of Cl xy in terms of set addition. The outermost polygon is #, the innermost is 
Cl^ j {B). The dashed polygons are copies of ( t)<> placed at vertices of B, therefore the convex hull of the 
inner polygon and these dashed polygons is Cl x A lJ (B)$i(A)o by Theorem 4. Note dial CJ^ y {13)Q)(A) ^ B, 
the shaded regions arc B — (CI X A V (B) © (A) ). 
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A related characterization of CI% JZ (B) is also possible. For c an (x,y t z) configuration, if c £ 
CI^ Z {B\ then c © (/l)o C B. Since CI^ Z {B) is the set of all such c, then CI^B) © (/l) C £ 
and furthermore CI% JZ (B) is the maximal such set, see (a) and (b) below. Clearly, if B = X (A) 
then X = CI% JZ (B), sec (c). Figure 8 illustrates these results. 

Theorem 3: For A and Z? convex polyhedra, 

(a) VI:ie(^) CB«XC C7^(i3) 

(b) C/f(B)0WoCB 

(c) cry z {co x g%B)) = B 

Note also that CXt(J3) = — CO A {— B), i.e. for ,4 to be inside B, it must be outside of S's 
complement 
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8. Algorithms for CO^ z {B) and CO*»{B) 

Theorem 4 provides a way of computing CO^ z (D) exactly for convex A and B. In addition, it 
provides an approximation technique for CO^ yz (B) when A and Z? are non-convex. 

Theorem 4: For polyhedral andB, 

cont?(/l © J3) = conv(A) © conufJS) = coni>(t>er£(i4) © i?er*(J3)) 
where cara^X) denotes the convex hull 7 of set X and veri(X) is die set of vertices of X. 

Proof: First show \hziconv{A © D) = conv(/l) © coni>(B), 

(2) 

The definition of convex hull states that any a £ conv(A) can he expressed as an affinc combina- 
tion of points in A. This may also be done for any b G conv(B). ]f z G corw(,4) conu(B), 
a G conv(/l), 6 G conu(B), a,- G X, 6; G B, £ f H = 1. 7; > 0, X^/?j = 1 and (3j > then 
x = a + b = ( J] 7,-a,-) + 6 = J] 7 / (o i + b) 



= E -* E ^ + ^ = E E t^k + **) 



2 J % J 

But, since £ z X^7i/?i = 1 and 7^ > 0, a: is an affinc combination of points in A © B and 
therefore belongs to its convex hull. Therefore conv(A) © conv(B) C conv(i4 ©£). 

(Q 

If a; G conu(/i ® 5), then for a, G /4 and 6; G B, 

* = E ^ + **•) = E ^°* + E t< 6 *'- 

i i i 

Therefore, x £ conv(A) © a>m>(B). 

This establishes that conv(A © B) = com;(/l) © com;(B). Replacing verf(i4) for A and verf(£) 
fori? and using the fact that conv(A) = conv(t;cri(i4)) [13], shows tiiat conv(vert{A) © t;erf(B)) = 
conv(i4) © conv(B). 
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7 The convex hull, conv(A), of a nonempty set A C 3^ is { ^^7,-Si I ^ £ /4,7» > 0, £?_ ^ = 1, n = 
1,2,...} [13p.l5] * *~ X 
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Corollary: For convex polyhcdra A and S, A © B = com;(t;er*(/l) © uer/(S)) and therefore 
CO;!^(B) - concert (J3) © vcr£((4) )). 

Proof: The first part of the corollary follows directly from the fact that, for convex A, A = 
conv(A). The second part follows from Theorem 2. 
I 

Many algorithms exist for finding die convex hull of a finite set of points on the plane, e.g. [9] 
[12] [15] [29]. [29] also describe an efficient algorithm for points in 5R 3 . These algorithms are known 
to run in worst-case time 0(v log v), where v is die size of the input set. Therefore, Theorem 4 leads 
immediately to an algorithm for CO xyz and an upper bound on die computational complexity of the 
problem. 

Theorem 5: For convex A } B C 5R 3 , CO xyz {B) can be computed in time 0(n 2 log n); where 
n = \vert(A)\ + \vert{B)\. 

Proof: The set vert{B) © vert((A) {) ) is of size \vert(A)\ X \vert(B)\, i.e. 0{n 2 ). Applying an 
0(v log v) convex hull algorithm to this set gives an 0(n 2 log n) algorithm for computing CO^ yz (B). 
This result holds only for convex polyhcdra of dimension k < 3 [7]. 
I 

The algorithm of Theorem 5 is not optimal; an 0(n) algorithm exists for CO% J (B) when A and B 
are convex polygons 8 : 

Definition: n(A } u) denotes the supporting plane (line) of A with outward normal u. *[A,u) 
contains at least one boundary point oM, call it a, and for any o! £ A then (a f } u) < (a, u). Thus, all 
of A is in one of the closed half spaces bounded by tt(A } u) and u points away from the interior oM. 

Lemma 1: If A andS are convex sets then 

tt(A ®B } u)f){A®B) = (44, u) n A) © (tt(B, u) O B) (1) 



8 The development in this section is based on that in Section 14 of [4] 
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Figure 10. Illustration for Lemma 1. 



Lemma 2: 

(a) Let s(aj, a 2 ) be a line segment and b a point, then s(aj, a. 2 ) © 6 = s(ai + 6, a 2 + 6) is a line 
segment parallel to s(a\ , a 2 ) and of equal length. See Figure 11(a). 

(f>) Let s(a u a 2 ) and s(b h b 2 ) be parallel line segments such that (a 2 — a x ) = fc(6 2 — &i) for 
k > 0. Then s(a b a 2 ) © s(6|, 6 2 ) = s(a { -f 6 b a 2 + 6 2 ) and the length of the sum is the sum of the 
lengths of die summands. See Figure 11(b). 

Theorem 6: For convex polygons A and B, CO% J (B) can be computed in worst case time 
0(\vert{A)\ + \vert{B)\). 



^"""S 



Proof: For fixed u, each term on the right hand side of (1) is either a line segment (edge) or a 
single point (vertex), it follows from Lemma 2 that the term on the left is one of: 

a. a new vertex, when two vertices are combined; 

b. a displaced edge, when an edge and a vertex are combined (Lemma 2a); 
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Figure 11. Illustration for Lemma 2. 



c. a pair of displaced end-to-end edges, when two edges are combined (Lemma 2b). 

As u rotates counterclockwise, the boundary of A ©5 is formed by joining a succession of these line 
segments. Note that, because of the convexity of A and J5, each edge is encountered exactly once [22 
p.13]. 

A polygon is stored as a list of vertices in die same order as they are encountered by the coun- 
terclockwise sweep of u. This is equivalent to a total order on the edges, based on the angle that the 
edge makes with die x axis. For a polygon P, assume the j th edge in this order, ej = s(v j} v j+ i), 
makes the angle Oj, then 



{v jt if Oj-i < 6{u) < 0j 

e J9 if 0(u) = 0j 

v j+u \FO j <0{u)<e j +i 

The time for constructing the new vertices is bounded by a constant, since it involves at most two 
vector additions. Thus A © B can be computed in linear time during a scan of die vertices of A and 
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Figure 12. The edges of /?Q(/1) , when A and /? are convex polygons, are found by merging the edge 
lists of B and Q(. \) , ordered on the angle their normals make with the positive x axis. 



B, Figure 12, An implementation of this operation is shown in Appendix 1. Similarly 00 (A)o can be 
computed in linear time by first converting each vertex a t to rv A — a t , Figure 12. 
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When ^ and/or B are non-convex polygons, CO% J (B) can be computed by an extension of 
the algorithm above. The method relies on decomposing the boundaries of the polygons into a 
sequence of polygonal arcs whose internal angles, i.e. the angle facing the inside of the polygon, 
are each less than 7r. The algorithm of Theorem 6 can then be applied to pairs of arcs; the result 
is a polygon whose boundary, in general, intersects itself. The algorithm requires, in the worst case, 
0{\edges{A)\ X \edges{B)\) steps [20]. 
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9. Dealing with Rotations of A 

If A and B are polygons, then CO A (B) is an object in Jl 3 , denoted CO% J °(B). The shape of 
CO*^°{B), when /i and B are convex, will be investigated below by examining changes in the cross- 
sections 9 ofCO% j6 as 6 changes. 

Assume a fixed value for 9. If A and B have no parallel edges, die proof of Theorem 6 shows that 
each edge of CO^ j {B) can be expressed as one of: 

e ( ] = b j ®s{a 1 {9la l ^{0)) (2a) 

*) = *i{0)®s{bj,b i+ i) (26) 

In these expressions, b 3 is the position vector of the j th member of vert(B) and a ? (0) is the position 
of the i th member of vert(Q{A)u\ which depends on 9. The order in which the a { and bj are 
encountered in the counterclockwise scan described in Theorem 6 determines the (i,j) pairings of 
vertices and edges. 

Equation (2) shows that, for small changes in 0, the ej rotate around bj, while the e h - are simply 
displaced, Figure 13. In addition to these changes in the zy-cross-section of CO^° 9 there are discon- 
tinuous changes at values of 0, denoted 0*, where an edge from A becomes parallel to one from J3. For 
values of just greater than these 0*, this pair of edges has a different order in the scan of Theorem 
6 from what they had when was just less than 0*. Therefore, die (i,j) pairings between edges and 
vertices changes. There are 0(\edges(A)\ X \edges(B)\) such 9* \r\CO x ^ e . 

Between discontinuities, die lines defined by e h - edges have a simple dependence on 6. The edge 
s i b j> b j+i) is on a l in c whose vector equation is: (x,Uj) = {b j} Uj} where Uj is the constant unit 
normal to s{bj, fy+i). Let a z (0) make the angle -f rji with the x axis, with ^ constant, and Uj make 
the angle <f>j with the x axis. Then, the equation for the line including e h - is 

(u jf x) = ( Uj , (ii{0) + bj) 

= ||a z || cos(0 + rji — <j>j) + (b j9 Uj) 

The terms are illustrated in Figure 14. This equation holds only for 9 £ \4>j — r\ u <£ i+1 — r) t ) between 
discontinuities; but within that interval it defines a plane in the space (a;, y, cos0). This space is 
analogous to that defined by semi-log graph paper. As long as die 0-interval is known and cos" 1 is 
9 The cross-section of CO x A yd {B), for constant 0, is CO x A y (B). 
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Figure 13. CO^{B) is the convex hull of the union of vcrt(f?(A) bi ) for each b z <E vert{B). When /4 
(and O(.-t)o) rotates by 0, the e n x rotate around fy and the ej are displaced. When an e£ is aligned with 
an e h 3 for some 0> any extra rotation will interchange the order in which they are encountered during the 
counterclockwise scan of Theorem 6. 
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single valued over the interval then the mapping from (z, y } cosO) to the (x, y, 0) space is unique. The 
0-ranges can always be chosen so that this is the case. 

The e a { cannot be treated in a similar fashion because the orientation of the edge changes with 0, 
i.e. the equation of the surface involve products of the form xzosO and ycos0. Instead of trying 
to represent them exactly by non-planar surfaces, this section develops a simple approximation tech- 
nique that avoids dealing directly with these edges. The technique will be illustrated first for CO xy 
and later for CO 3 * 6 . 

For fixed 0, if the lines defined by the e b j are extended until they intersect, the resulting figure 



23 



Dealing with Rotations of A 




/*~\ 



Figure 14. Illustration of terms in equation (3). 
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completely includes CO^ y (B). This approximation can be very poor when the angle defined by ad- 
jacent ej- is very acute, sec Figure 15. The approximation can be improved by introducing additional 
lines whose normals point between those of e b j _ 1 and ej. These lines should be farther from bj than 
any of the t\ paired with bj in (2a). 

This method can also be used to approximate the slice of CO% j0 (B) between discontinuities. The 
e b j and the lines to bound die t\ edges both define planes in (x, y, cos£) space, see (3), for some range 
of 0-values. The boundaries of the ^-intervals also define planes whose equations are of the form 
== <t>j~ *li. These planes bound half-spaces whose intersection defines a convex polyhedron in 
(x, y } cos#). This polyhedron contains all of CO x ^° within the ^-interval. Therefore, it can be used 
to approximate CO% j0 over that interval. The union of the resulting convex polyhedra for each 0- 
interval is an approximation to CO x ^ h \ 

The discussion above shows how to build a set of n = \edges(B)\ + \vert(B)\ half spaces 
bounding CO% j0 (B) within each 0-range. These half spaces can be intersected to construct a convex 
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Figure 15. Approximating CO T ^{B) using only the e) works well at vertices with large interior angle, but 
poorly for small interior angles. The lines shown dashed can be used to improve the approximation. 



polyhedron in 0{n log n) time [7] [30]. There are 0(\edges(A)\ X \edges[B)\) 0-rangcs that need to 
be considered; therefore, the complete approximation may he found in 0(?i 3 log n) time, although in 
many applications, a complete approximation might not be necessary. 

The same techniques can also be applied to computing CFf{B) and since the CI only has edges 
of the form (26), the resulting polyhedra in (x, y, cos#) arc an exact representation of the Cspace A 
entity [20]. 

This approximation is, in principle, applicable to polyhedra in 3-dimensions; the results would be 
a set of polyhedra in 6-dimcnsions. The extension is conceptually simple, but die difficulties of deriv- 
ing and representing die three-dimensional orientation constraints make die approach unattractive. 
The next section examines an alternative to dealing with die high-dimensional polyhedra required by 
this technique. 
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10. Approximating High Dimension C space Obstacles 

Section 3 introduced die use of projections of obstacle slices as a means of approximating high 
dimensional C space Obstacles. Figure 16 shows a decomposition of Cspace A C 3J 3 into a family 
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Figure 16. z slice projections. This example shows the slice ""projections decomposing CspaceA 
into four Cspace A [7,, 7j+i). 
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of slices Cspace^j^j+i]. Each of these new Cspaces is a z slice projection of C space A . This 
new family of spaces is a conservative representation of the obstacles, i.e. it represents the worst case 
constraints on configurations of A whose z coordinates are within the range fy, 7/4-1]. 

The power of the slice projection approach is that the family of slices captures all the constraints 
needed to plan safe paths in Cspace A . Having all the slices of the CO A {B{\ there is no need to 
refer to the CO A (Bj) themselves. On the other hand, the algorithms in the Sections 6 through 9 are 
for cross-sections of CO A {B), e.g. CO% J and CO T £\ not for slice projection. The basic result of 
this section is that these algorithms can be used to compute polyhedral approximations to die slice 
projections. 

The construction that relates slice projections to cross-section projections is the swept volume of an 
object. Intuitively, the swept volume of A is all the space that 4 covers when moving within a range of 
configurations. In particular, given two configurations for A, called c and c', then the union of (A) a for 
all c < a < d is the swept volume of A over the configuration range [c, d). Generally, c and d differ 
/~\ only on some subset, K\ of the configuration coordinates. For example, if c and d are of the form 

(/?!, /?2, #3) and K = {3}, then the swept volume of A over the range [c, d\ K refers to the union of A 
over a set of configurations differing only on (3 3 . The swept volume of A over a configuration range is 
denoted >1[c,c / ]k'. 

The swept volume of, 4, a rigid object, is also a rigid object 10 with the same number of degrees of 
freedom. For linked polyhedra, the situation is not so simple, because of the interdependence of the 
C space parameters. 

Note that for a linked polyhedron, the position of link j typically depends on the positions of 

links k < j, which are closer to the base than link j. Let K = {j}, c = (0 t -), d = {6[), and [c, d] K 

define a range of configurations differing on the j th Cspace A parameter. Since joint j varies over a 

range of values, links I > j will move over a range of positions which depend on the values of c and 

c', as shown in Figure 17. The union of each of the link volumes over its specified range of positions 

is the swept volume of the linked polyhedron. The swept volume of links j through n can be taken 

as defining a new j th link. The first j — I links and die new j fh link define a new manipulator whose 

configuration can be described by the first j — 1 joint parameters. On the other hand, the shape of the 

^m^ ]0 Note that, in general, the swept volume of a polyhedron is not a polyhedron, although the development relies on 

computing polyhedral approximations to it. 
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Figure 17. Changes in the second joint angle from 2 to 0' 2 causes changes in the configurations of both link 
A 2 and link A 3 . 



new link j depends not only on die A'-parametcrs of c and c', i.e. 9j and 0'., but also on t for I > j. 
This implicit dependence on parameters of c and c f that are not in K is undesirable, since it means 
that die shape of the new j ih link will vary. Letting K = {]',..., n}, then the shape of the swept 
volume depends only on the /^-parameters of c and c ; , while its configuration is determined by the 
{I — /O-parainctcrs. A swept volume that satisfies this property is called displaceable. 

Hie fact that the swept volume of a linked polyhedron A does not have the same degrees of 
freedom as does A forms the basis for the relationship between slice projection and cross-section 
projection. If the swept volume is displaceable, the I — K parameters may be changed, but changes 
to die K parameters arc not legal. Therefore, the C 'space of the swept volume of A is of lower 
dimension than die C space of A. In particular, configurations in Cspace A that have equal I — K 
parameters and whose K parameters are in die defining range of die swept volume, project into the 
same configuration in Cspace^ c ^ K . 

If A[c } d\ K overlaps some obstacle B then, for some configuration a in that range, (A) a overlaps 
B. The converse is also true. IM[c, c r \ K is displaceable, then CO A[C)C t ]K {B) is die set of 7 — K projec- 
tions of diose configurations of A within [c, d\ K for which A overlaps B, Equivalent^, CO A[cd]K (B) 
is die I — K projection of the [c, c f ] K slice of CO A (B). If the configurations of the swept volume are 
one of (x, y\ (x, y, z\ or (x, y, 0) then the algoridims of the previous sections can be used to compute 
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CO-\[c,c'] K [B) and thereby compute the required slice projections 11 . 

A formal statement and proof of this result is included in Appendix 2 as Theorem 8. This theorem 
is of practical importance since it provides the mechanism underlying the Findspacc and Findpath 
implementations described in [20] and [21]. In addition, the proof of the theorem demonstrates the 
usefulness of the C 'space concept as a tool in theoretical analyses of spatial planning problems. 



11. Related Work in Spatial Planning 

The definition of the Findspacc problem used here is based on that in [46]. Approaches to this 
problem are described by [37] and [27], The latter, which is the more relevant, is an application of the 
Warnock algorithm for hidden line elimination. It involves recursively subdividing the workspace un- 
til an area "large enough" for die object is found. This approach has several drawbacks: (1) any non- 
overlapping subdivision strategy will break up potentially useful areas, and (2) the implementation of 
/•-% the predicate "large enough" is not specified. 

The C space approach to Findspacc and Findpath described here is an extension of that reported 
in [21]. In that paper, an approximate algorithm for CO xyz {B) is described and the Vgraph algorithm 
for high-dimensional Findpath is first presented. 

The basic idea of representing position constraints as geometric figures, e.g. CO xy (B), has been 
used (independently) in [1], [2] and [3], who employed an algorithm to compute CO xy for non-convex 
polygons in a technique for two dimensional layout. The template packing approach described in 
[10] uses a related computation based on a chain-code description of figure boundaries. [36] reports 
algorithms for packing of parallelopipeds in the presence of obstacles using a construct equivalent to 
die CO xy , but defined as "the hodograph of the Close Positioning Function". The only use of this 
construct in the paper is for computing CO xy for aligned rectangular prisms. 

The work by Udupa, reported in [40] [41], was die first to approach Findpath by explicitly using 
transformed obstacles and a space where the moving object is a point. Udupa used only rough ap- 
proximations to the actual C space obstacles and had no direct method for representing constraints on 

n Of course, this requires computing a convex polyhedral approximation to the swept volume of A. Simple approximations 
are not difficult to compute [20], but this is an area where better algorithms arc required. Nevertheless, the swept 

f*\ volume computation is a 3-dimensional operation which can be defined and executed without recourse to 6-dimensional 

1 ' constructs. 
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more than three degrees of freedom. [41] also surveys previous heuristic approaches to the Findpath 
problem for manipulators [17] [28] [44]. An early paper on Shakey [25] describes a technique for 
Findpath using a simple object transformation that defines safe points for a circular approximation 
to the mobile robot and uses a graph search formulation of the problem. More recent papers on 
navigation of mobile robots are also relevant to 2-dimensional Findpath [11] [23] [39]. [14] reports on a 
program for planning the path of a 2-dimensional sofa through a corridor. This program does a brute- 
force graph search through a quantized Cspace. 

[31] proposes an extension of the approach in [21] to the general Findpath problem, but using 
an exact representation of the high-dimensional Cspace obstacles. The basic approach is to define 
the general configuration constraints as a set of multinomials in the position parameters of A. But, 
die proposal still requires elaboration. It defines the configuration space constraints in terms of the 
relationships of vertices of one object to die feces of die other, This is adequate for polygons, but 
the equations in die paper only express the constraints necessary for vertices of A to be outside of B, 
/~\ i.e. dicy are of the form of (3) above. They do not account for the positions of A where vertices of 

B are in contact with /4. Thus, the equations do not represent the correct constraints on the position 
of A. The new equations will have terms of die form xcosQ and y cos 0. Furdiermorc, die approach 
of defining the configuration constraints by examining die interaction of vertices and faces does not 
generalize to 3-dimensional polyhcdra. It is not enough to consider die interaction of vertices and 
feces; die interaction of edges and faces must also be taken into account [6]. 
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Appendix 1. Algorithm for (70^(5) 

This appendix shows an algorithm for ,4 © B y SET-SUM(A , B), when A and B are convex 
polygons. Section 5 shows how this operation can be used to compute CO xy . 

Each polygon is described in terms of its vertices and the angles that the edges make with the 
positive x axis. Hie edges and vertices are ordered in counterclockwise order, i.e. by increasing angle. 
The polygon structure in the following program has die following components: 

1. s i ze — number of edges in polygon. 

1 vert [0 : s ize] — an array of vectors representing the coordinates of a vertex. The 
i th edge, i = l,...,size, has die endpoints vert[i-l] and vert[i]. Note that 
vert[0] = vert[size]. 

3. angle [0:size] — the angle that the normal of an edge makes with the x axis, 
monotonically increasing. For convenience angle[0] = angle[size]. 

The algorithm follows directly from Theorem 6, noting that in this representation of polygons, 
edges are represented by successive vertices. 

SET-SUM (a, b) 

{ c = new-polygon (a. size + b.size); /* create new polygon of max size */ 
ea = 1; eb = 1; vc = 0; ang = 0; offset = 0; 
do { ea = ea + 1 } 

until (a.angle[ea] >= b.angle[l] 

and a.angle[ea - 1] <= b.ang!e[l]) 
c.vert[0] = a.vertfea] + b.vert[0]; 
do { vc = vc + 1 ; 

ang = offset + a.angle[ea]; 
if (ang <= b.angle[eb]) 

then if (ea > a. size) /* handle wraparound */ 

then { offset = 2pi; ea = 1; } 
else ea = ea + 1; 
if (ang >= b ,angle[eb]) 

then eb = eb + l; 
c.vert[vc] = a.vertfea] + b.vert[eb]; 

} 
until (ea = a. size and eb = b.size); 
c.size = vc; 
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Appendix 2. Proof of Theorem 8 

Assume that Cspace^ C 3^, let / = {1, 2, . . . , d} and tf C L /, K andl — K shall denote 
sets of indices for the coordinates of a G Cspace A . Define the following vectors, all in Cspace A ; 
b - (&), c = ( 7i ) and c' = (7'.) for i G L Then, 

* /c (c) = 4>/<(c, c) 
G K (c ? </) = * K (c, <0 fl */-./r(c, c) 

These definitions are illustrated in Figure A18. 

The projection operator, denoted P K [ • \M d h-+ Rl^ I is defined, for vectors and sets of vectors, by: 

PK[b] = (p k ) kGK 

PK[B) = {P K [b)\bGB} 



/-N. 
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Superscripts on vectors indicate projection, e.g. b K — Pj<[b}. In addition, the vector in JR' 7 ' com- 
posed from one vector in 3ft |/v| and one in ^ f ~ K \ is denoted {a J ~~ K : b K \ where F/_ A [ (a I ~ K : 6 ;< ) ] = 
a 1 "* and P A [ (a 7 "* : b K ) } = b K . 

In this notation, precise definitions for the notions of cross-section projection and slice projection 
can be provided. The cross-section projection of a C space A obstacle is written as follows: 

CO A (B)[c) K = Pj_ K [CO A (B) n */c(c) ]. 

The slice projection, is similar to the cross-section projection, but carried out for all configurations 
between two cross-sections: 

co A {B)[c % d] K = Pi-K[co A {B)n* K (c,cri]. 

The /^-parameters of the two configurations, c and c', define die bounds of the slice. Similarly, the 
swept volume can be defined in this notation: 

Definition: The swept volume 12 of A over the configuration range [c, d] K is 

(A[c t cf] K ) c ~ (J (A) a 

The requirement discussed in Section 10 that the swept volume of A be displaceable is embodied 
in the following condition: 

Va: |J {A)( a i-K :X K } = (Afacffc^ai-K.^ (4) 

Note that the I — K parameters may be changed, as in (4), but not those parameters in K. Therefore, 
(A[c, c f )is) a is defined only if a £ $k(c). 

Lemma 3: If (4) holds, i.e. if the swept volume is displaceable, then 

Pi- K [co A[Cid]K {B) n */c(c)] = Pj- k [(co a (b) e e^o, e - c)) n **(*)]. 



/***N. 



12 The similarity in the notation between swept volume and slice projection does not imply any direct relationship. 
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Proof of Lemma: 

Q) 

If o G Pi- K [(CO A {B)Q& h {Q, c'— c))n*/v(c)], then there exists an {a l ~ K : x><) G CO A (B) 
and an x 2 G @a (0, c' — c) such that zf — a;^ = c ;< . This implies that zi G $k -(c, c'). Then, using 
(4), 

{A\ a l-K . X K) C [A\c, c') K \ a l-K . c Ky 

But since {a r ~ K : if) G CO„i (J3), then (/!)(„;-* . x k } intersects B; therefore, its supersets also inter- 
sect B. By the definition of CO 

(a ! - K : c K ) G CO A[( , c , ]K (B) f) *,<•(«) => a £ Pj_«\CO a[cx , ]k {B) n 4>/<(c)]. 

(C) 

Assume o G ?7_/c[C0. 1[C|( ., ]k (B) n *a-(c)]; then, 

/""% I' 4 [ C > C ' ]tf)(a'-* : C*) n B y£ 

For all x G e K (0, c' - c), (a'-* : c*) + * G <Mc, c 7 ) and P,_ K [ (a 1 ^ :c K ) + x]= a ! ~ K . 
Therefore, by (4) 

(A\ a l-K. C K) +X C {A\c,d) K \ a l-K. c Ky 

Since these are all me sets that make up the swept volume, at least one of them must also overlap B 
when the swept volume does; therefore tliere must be some x x G Q/<(0, d — c) for which 

{A\ a i-K. c K )+Xl C\By^9. 

By the definition of CO, 

(a 1 -* : c K ) + x x £ CO A (B) « (a 1 -* : c K ) G (CO A (B) - 9 A (0, c' - c)) n */<(c). 

Clearly, then 

a G />,_*[ (CQ4(B) - e K (0, d-c))n * K {c)] 



/-> 



This Lemma leads to a proof of Theorem 8. 
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Theorem 8: If (4) holds, then 

p 7 _*[ co a (b) n * k ( C| cO ] - P/-/W co . xlCl&]K (B) n **(c) ] 

Proof of Theorem: 

The proof below shows that 

P/_/v[ CO x (B) n *k(c, ] = P 2 _ K [(CO A (B) 7 , -(0, d - c)) *k(c) ]. 
Hie theorem follows directly from tliis result and Lemma 3. 

(Q 

If a G jP,_ k [ CO. { {B) n 4> /v (c, d) ], then, for some x G K (O, c' — c) there is an a! such that 
ay = (a 7 - ;< : c*' + x*) G CO A (B) *k(c, c') 
f*"- Since x 1 ~ K — 0, then ay — x E $k (c) and therefore 

a, - x G (CO A {B) 0,(0, d - c)) f) * K (c). 
Since a = P;_/ v -[a] — x] 

a E P,- K [{CO A [B) 0,(0, d - c)) $k(c) ]. 

(2) 

Assume a G P/_. A [ (C0. 4 (J3) 0,<(O, </ - c)) n *k(c) ], then, 

(a 7 - ;< : c ;< ) G (CQ,(B) Q & h {0, d - c)) * K (c). 

Since the 7 — K parameters are not changed by the set subtraction, there must be an ay == 
{a I ~ K : zf ) G CO A (B). x\ must be in */<•(<:, c'), because P K \xy — x 2 ] = c with x 2 G ©k(0, c 7 — c). 



(a'~ K : **) G CO^B) D *k{c, d) => a G Py-KfCO^B) n */<(c, (/)]. 



/**\. 



